#include "WASP.h"


Wasp::Wasp(Fractal* f, float step) :m_Graph(f->GetGraph())
{
	
	m_pFractal=f;
	m_stepSize=step;
	m_gridSize = f->GetGridSize();
	m_numNodes = m_pFractal->NumOfNodes();
	m_vNodes.reserve(m_numNodes);
	for (unsigned int i=0; i<m_numNodes;++i)
	{
		m_vNodes.push_back(D3DXVECTOR2(i%m_gridSize,i/m_gridSize));
	}
}

bool Wasp::GenerateWeights()
{
	
	float weight; //weight of using the path
	float curHeight, destHeight; // the height of this point and the height of the destination to check
	D3DXVECTOR2 offset[8]; //position offset from this node
	GraphNode* curNode,* destNode; 
	D3DXVECTOR2 curPos, destPos;
	int from,to; //Node Ids
	int stepsx, stepsy;
	float pyA,pyB,pythag; //Used to calculate pythagarian distance between points

	for (unsigned int i=0; i<m_numNodes;++i)
	{
		//establish positons of possible paths
		curNode = new GraphNode(m_vNodes[i]);
		curPos = curNode->GetPos();
		curHeight = m_pFractal->GetPoint(curPos);
		offset[0] = curPos+D3DXVECTOR2(0,1); //N
		offset[1]= curPos+D3DXVECTOR2(1,1); //NE
		offset[2]= curPos+D3DXVECTOR2(1,0); //E
		offset[3] = curPos+D3DXVECTOR2(1,-1); //SE
		offset[4] = curPos+D3DXVECTOR2(0,-1); //S
		offset[5] = curPos+D3DXVECTOR2(-1,-1); //SW
		offset[6] = curPos+D3DXVECTOR2(-1,0); //W
		offset[7] = curPos+D3DXVECTOR2(-1,1); //NW

	

		for(unsigned int i=0;i<8;++i) //hard coded 8 as there will only ever be 8 possible directions on a compass
		{
			//used to make code into a loop
			destNode=new GraphNode(offset[i]);
			destPos = destNode->GetPos();
			//check if this is a valid path
			if((destPos.x>=0 && destPos.x<=(m_gridSize-1)) && (destPos.y>=0 && destPos.y<=(m_gridSize-1)) )
			{

				//get height of destination
				destHeight = m_pFractal->GetPoint(destPos);
				//calclate absolute difference between heights
				weight = fabs(curHeight-destHeight);

				//add absolute distance between position
				stepsx = curPos.x - destPos.x;
				stepsy = curPos.y - destPos.y;

				pyA=stepsx*m_stepSize;
				pyB=stepsy*m_stepSize;
				pythag = sqrt((pyA*pyA)+(pyB*pyB)); //a squared + b squared = c squared

				weight+=pythag;
				
				//add these edges and weights to the graph
				from = curPos.x + (curPos.y*m_gridSize);
				to = destPos.x + (destPos.y*m_gridSize);
				m_Graph.AddEdge(GraphEdge(from,to,weight));
				m_Graph.AddEdge(GraphEdge(to,from,weight));
			}
		}
	}
	return true;
}

std::vector<D3DXVECTOR2> Wasp::FindPath(D3DXVECTOR2 Start, D3DXVECTOR2 End)
{
	std::vector<D3DXVECTOR2> res;
	int s = Start.x + (Start.y*m_gridSize);
	int e = End.x + (End.y*m_gridSize);
	m_Search = new AStar(m_Graph,s,e);
	std::list<int> path =m_Search->Path();
	while(!path.empty())
	{
		int x = path.front()%m_gridSize;
		int y = (path.front()-x)/m_gridSize;
		res.push_back(D3DXVECTOR2(x,y));
		path.pop_front();
	}
	return res;
}